#coding: utf-8 

import pygame

from math import atan2,sin,cos
from auxMath import *
from random import *
from gameUtils import *
from position import *
from cleanArea import *
from simData import *

CONSUMPTION_UNIT = 100.0/5000.0
DEBUG = True

CLEANSIM_MODEL=0
ROMBA_MODEL=1

class Robot(pygame.sprite.Sprite):
	u"""Class derivada do pygame.sprite.Sprite e como tal tem a capacidade
	de se desenhar numa frame.
	Esta classe representa um Robot do sistema.

	Contem as variáveis:

	images - representações
	
	batery - bateria (em percentagem)
	dirt - lixo (em percentagem)
	
	canbeblocked: Robots bloqueiam-se mutuamente - impedindo colisões
	target - lista de posições a ocupar pelo Robot
	id - número identificador do Robot
	
	BATERY_MAX = 100
	MOVE_CONSUMPTION*BLOCK_SIDE/KIVASPEED=X
	TURN_CONSUMPTION*BLOCK_SIDE/KIVASPEED=X/2
	
	BATERY_MAX dá para 5Km= 5000X
	
	CONSUMPTION_UNIT = X = 100/5000 = 0.02
	
	"""
	global CONSUMPTION_UNIT
	vMax=1.0
	wMax=1.0 #turns around himself/second
	v=0.0
	w=0.0
	
	TETA_INC=1./16
	D_INC=0.1
	
	BATERY_MAX = 100.0
	MOVE_CONSUMPTION=CONSUMPTION_UNIT *vMax 
	TURN_CONSUMPTION=CONSUMPTION_UNIT /2*wMax
	
	images=[]
	canbeblocked=True
	serial=0
	
	arranca=True
	notarget=False
	
	isBeingControledBy = None
	
	def __init__(self,parent,x,y,angle,scale,floor,robotModel):
		u"""Inicializador da classe Kiva"""
		
		# Import Psyco if available
		try:
			import psyco
			psyco.full()
			psyco.log()
			psyco.profile(0.05)
		except ImportError:
			pass
		
		id=Robot.serial
		Robot.serial+=1
		self.robotModel=robotModel
		
		
		
		pygame.sprite.Sprite.__init__(self, self.containers) # Sprite inicialization
		
		self.floor=floor
		self.pos=Position(x,y)
		
		if self.robotModel==0:
			self.cleanAreaTree=CleanAreaTree(2)
			self.cleanAreaR=2
			self.cleanArea=CleanArea(self.cleanAreaR)
			self.cleanEscovasTree=CleanAreaTree(1)
			self.cleanEscovas=CleanArea(1)
		else:
			self.cleanAreaTree=CleanAreaTree(2)
			self.cleanAreaR=2
			self.cleanArea=CleanArea(self.cleanAreaR)
			self.cleanEscovas=CleanArea(0)
		
		self.scale=scale
		self.parent=parent
		
		self.target=[]
		self.angle=angle
		self.id=id
		self.batery=100.0
		self.dirt=0.0

		self.rised=False
		self.stoped=False
		
		
		self.original=self.images[self.robotModel]
		self.image = self.original
		self.rect = self.image.get_rect()
		
		self.orders=[]
		self.currentOrder=0
		self.orderInitTime=0.0
		self.clock=0.0
		
		self.targets=[]
		self.update()
	@property
	def get_floor(self):
		return self.parent.building.floors[self.floor]
	
	@property
	def last_target(self):
		try:
			order = self.orders[len(self.orders)-1]
			targets = order[1]
			#~ print "self.orders",
		except:
			targets = self.targets
			#~ print "self.targets"
		try:
			#~ print targets
			target = targets[len(targets)-1]
			#~ print "target"
		except:
			target = self.pos
			#~ print "self.pos"
		return target
	
	def isFree(self):
		return len(self.targets)==0 and len(self.orders)==self.currentOrder
	
	def addOrder(self,name,targets):
		self.orders.append((name,targets))
		if (self.currentOrder+1==len(self.orders)):
			self.executeNextOrder()
		
	def orderDone(self):
		if(len(self.orders)>self.currentOrder):
			execTime=self.clock-self.orderInitTime
			if self.orders[self.currentOrder][0] != None:
				print 'Order Solved:', self.orders[self.currentOrder][0], 'in %.2fs' %(execTime),'.'
			self.currentOrder+=1
			self.lastEval=execTime
			if self.isBeingControledBy is not None:
				self.isBeingControledBy.searchAC.evaluate(execTime)
		self.executeNextOrder()
	
	def executeNextOrder(self):
		if(len(self.orders)>self.currentOrder):
			self.targets=[]
			self.addt(self.orders[self.currentOrder][1])
			self.orderInitTime=self.clock
			
	
	def __repr__(self):
		u"""Funcao para representar o Robot.
		Usage: print robot"""
		rep= "Robot " + str(self.id)+" : "
		rep+=" ; pos" + str(self.pos) + " ; angle"+ str(self.angle)
		rep+=" ; batery("+ str(int(self.batery))+"%)"+" ; dirt("+ str(int(self.dirt))+"%)"

		return rep
	
	def stop(self):
		self.stoped=True
	
	def run(self):
		self.stoped=False
		
		
	def useEnergy(self,value):
		u"""
		Robot consome energia.
		Value é dado em percentagem da bateria.
		É consumido um valor entre [0,value].
		"""
		if self.batery<value:
			self.emptyBatery()
			return False
		else:
			self.batery-=value*random()

			return True
		
	def generateEnergy(self,value):
		u"""
		A bateria do Robot é recarregada
		"""
		if self.batery+value>self.BATERY_MAX:
			self.fullBatery()
			return False
		else:
			self.batery+=value*random()
			return True
	
	def fullBatery(self):
		u"""Bateria foi recarregada."""
		self.batery=self.BATERY_MAX
		self.gotoTarget()

	def emptyBatery(self):
		u"""Bateria chegou ao fim"""
		self.batery=0

	def bateryIsEmpty(self):
		u"""A Beretia está vazia?"""
		return self.batery<=0

	def bateryIsLow(self):
		u"""A Beretia está abaixo de 20%?"""
		return self.batery<20.0
		
	def turn(self,teta):
		self.angle=normalize(self.angle+teta)
	
	def left(self,teta):
		self.turn(teta)
		
	def right(self,teta):
		self.turn(-teta)
	
	def forward(self,d):
		self.pos=self.pos.add_dist(d,self.angle)
	
	def backward(self,d):
		self.pos=self.pos.add_dist(-d,self.angle)
		
	def go_to_teta(self,teta):
		dteta=normalize(teta-self.angle)
		if abs(dteta)<=self.TETA_INC:
			self.turn(dteta)
			return True
		else:
			self.turn(dteta/abs(dteta)*self.TETA_INC)
			return False
			
	def go_in_front(self,d):
		if d>0:
			if d>self.D_INC:
				step=self.D_INC
			else:
				step=d
			self.forward(step)
			return d-step
		else:
			return 0.
			
	
	
	
	def go_to_point(self,pos):
		desired_angle=self.pos.angle_to(pos)
		
		self.go_to_teta(desired_angle)
		
		dteta=normalize(desired_angle-self.angle)
		
		#se andar em frente aproximar o robot do target
		if abs(dteta)<pi/2:
			dist=self.pos.dist_to(pos)
			if abs(dteta)<dist/4.0:
				self.go_in_front(dist)
		
		dist=self.pos.dist_to(pos)
		return dist
		
	def move(self,dt):
		u"""Robot anda para a frente com velocidade = v"""
		
		# Batery consumption
		if not self.stoped:
			if self.useEnergy(self.MOVE_CONSUMPTION):
				
				self.angle += pi*self.w*dt
				self.pos.x += cos(self.angle) * self.v*dt
				self.pos.y += sin(self.angle) * self.v*dt 
				self.angle += pi*self.w*dt
	
	def move_to_targets(self):
		if len(self.targets)>0:
			#print 'era',self.targets[0][1],
			
			function=self.targets[0][0]
			param=self.targets[0][1]
			
			if function.__name__ == 'go_to_teta':
				targetDone=function(param)
			elif function.__name__=='go_in_front':
				remain=function(param)
				self.targets[0][1]=remain
				if remain==0:
					targetDone=True
				else:
					targetDone=False
			elif function.__name__ == 'go_to_point':
				dist=function(param)
				if dist<0.1:
					targetDone=True
				else:
					targetDone=False
				
			else:
				print 'ERROR: unknown Target'
				targetDone=True
			
			
			#print 'function',self.targets[0][0].__name__,' remain',self.targets[0][1]
			if targetDone:
				self.targets.pop(0)
			return True
		else:
			return False
	
	def addt(self,poss):
		self.addTarget_GoToPoints(poss)
			
	def addTarget_GoToPoints(self,poss):
		for pos in poss:
			self.addTarget_GoToPoint(pos)
			
	def addTarget_GoToPoint(self,pos):
		self.targets.append([self.go_to_point,pos])
	
	def draw_update2(self):
		if self.bateryIsEmpty():
			pass
		if self.stoped:
			pass 
			
			
		# Vai buscar a imagem e roda-a
		self.image=pygame.transform.rotate(self.original, self.angle*180/pi-90)
		
		#faz resize da imagem
		aux=self.parent.ZOOM*PIXEL2M
		size = self.image.get_size()
		self.image = pygame.transform.smoothscale(self.image, (int(float(size[0])/self.scale*aux),int(float(size[1])/self.scale*aux)))
		
		
		#vai buscar o rect correspondente à imagem
		self.rect=self.image.get_rect()
		
		#move o centro do rect para a origem
		newpos=self.rect.move((-self.image.get_width()/2,-self.image.get_height()/2))
		self.rect=newpos
		
		#move o centro do eixo do robot para a origem
		if self.robotModel==1:
			d=1.0/4.0*aux*1.1
		else:
			d=1.0
		newpos=self.rect.move(((+d*cos(self.angle)),-d*sin(self.angle)))
		self.rect=newpos
		
		#desloca o robo para a posição actual
		altura=self.parent.building.floors[self.floor].getAltura()*aux
		newpos=self.rect.move((self.pos.x*aux,altura-self.pos.y*aux))
		self.rect=newpos
	
		#~ Rp(q) = R(q) + p - R(p)
		#~ 
		#~ So rotating around p is the same as rotating around the origin and
		#~ then translating by p - R(p).

	def draw_update(self):
		#~ print self, "updated"
		
		if self.bateryIsEmpty():
			pass
		if self.stoped:
			pass 
			
		rect = self.original.get_rect()
		size = rect.size
		aux=self.parent.ZOOM*PIXEL2M
		
		imageO = pygame.transform.smoothscale(self.original, (int(float(size[0])/self.scale*aux),int(float(size[1])/self.scale*aux)))
		rect = imageO.get_rect()
		
		
		oldCenter = rect.center
		
		if self.robotModel==0:
			oldCenter=(rect.centerx,rect.centery-int(340.-454./2))
		
		angle=self.angle*180./pi-90.
		self.image=pygame.transform.rotate(imageO, angle)
		self.rect = self.image.get_rect()
		self.rect.center = oldCenter
		
		
		altura=self.parent.building.floors[self.floor].getAltura()
		newpos = self.rect.move((self.pos.x*aux,(altura-self.pos.y)*aux))
		
		self.rect=newpos
		
		posRelative2center=(-oldCenter[0]*(1),-oldCenter[1]*(1))
		
		newpos = self.rect.move(posRelative2center)
		self.rect = newpos
	
	
	def update(self):
		u"""Actualiza a cena para a pintar na imagem."""
		self.clock+=0.250
		self.move(0.040)
		if (not self.move_to_targets()):
			self.orderDone()
			self.executeNextOrder()
		
		try:
			cleanerPos = self.pos.add_dist(0.5-0.25,self.angle)
			robotMoved = cleanerPos.dist_to(self.lastPos)>0.1
			#~ print self.pos.dist_to(self.lastPos)
		except:
			robotMoved = True
		
		
			
		if not FASTMODE and robotMoved:
			self.lastPos = self.pos.add_dist(0.5-0.25,self.angle)
			self.lastAngle = self.angle
			if self.robotModel==0:
				#~ self.get_floor.dirt.clean_area(self.pos.add_dist(0.5,self.angle),self.cleanArea)
				#~ self.get_floor.dirt.clean_area(self.pos.add_dist(0.7,self.angle).add_dist(0.2,self.angle-pi/2),self.cleanEscovas)
				
				self.get_floor.dirt.clean_area_tree(self.pos.add_dist(0.5-0.25,self.angle),self.cleanAreaTree)
				self.get_floor.dirt.clean_area_tree(self.pos.add_dist(0.7-0.25-0.1,self.angle).add_dist(0.2-0.1,self.angle-pi/2),self.cleanEscovasTree)
				
			else:
				#~ self.get_floor.dirt.clean_area(self.pos,self.cleanArea)
				self.get_floor.dirt.clean_area_tree(self.pos,self.cleanAreaTree)

	
	
